import math


def spherical_to_cartesian(lon, lat):
    """将经纬度（弧度）转换为笛卡尔坐标（单位向量）"""
    x = math.cos(lat) * math.cos(lon)
    y = math.cos(lat) * math.sin(lon)
    z = math.sin(lat)
    return [x, y, z]


def cartesian_to_spherical(p):
    """将笛卡尔坐标（单位向量）转换为经纬度（弧度）"""
    x, y, z = p
    lon = math.atan2(y, x)
    lat = math.asin(z)
    return lon, lat


def vector_dot(v1, v2):
    """向量点积"""
    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]


def vector_cross(v1, v2):
    """向量叉积"""
    return [
        v1[1] * v2[2] - v1[2] * v2[1],
        v1[2] * v2[0] - v1[0] * v2[2],
        v1[0] * v2[1] - v1[1] * v2[0]
    ]


def vector_norm(v):
    """向量模长"""
    return math.sqrt(v[0] ** 2 + v[1] ** 2 + v[2] ** 2)


def normalize_vector(v):
    """向量归一化"""
    n = vector_norm(v)
    return [v[0] / n, v[1] / n, v[2] / n] if n > 0 else v


def great_circle_distance(p1, p2):
    """计算两点间的大圆距离（弧度）"""
    dot_val = vector_dot(p1, p2)
    dot_val = max(min(dot_val, 1.0), -1.0)  # 避免浮点误差
    return math.acos(dot_val)


def get_tangent_points(A, C, r_rad):
    """计算从点A到圆心C、半径为r_rad（弧度）的圆的切点"""
    dot_AC = vector_dot(A, C)
    dot_AC = max(min(dot_AC, 1.0), -1.0)  # 避免浮点误差
    d_AC = math.acos(dot_AC)
    sin_d_AC = math.sin(d_AC)

    if d_AC <= r_rad:
        raise ValueError("发射点在禁飞区内")

    # 计算向量 U = (A - (A·C)C) / sin(d_AC)
    U = [A[i] - dot_AC * C[i] for i in range(3)]
    U = [u / sin_d_AC for u in U]  # 归一化

    # 计算法向量 N = (A × C) / sin(d_AC)
    N = vector_cross(A, C)
    N_norm = vector_norm(N)
    if N_norm < 1e-12:
        # 处理A和C平行的情况（通常不会发生）
        raise ValueError("A和C平行，无法计算切点")
    N = [n_i / N_norm for n_i in N]

    # 计算旋转角
    alpha = math.acos(r_rad / d_AC)

    # 计算两个切点方向
    V1 = [
        math.cos(alpha) * U[0] + math.sin(alpha) * N[0],
        math.cos(alpha) * U[1] + math.sin(alpha) * N[1],
        math.cos(alpha) * U[2] + math.sin(alpha) * N[2]
    ]
    V2 = [
        math.cos(alpha) * U[0] - math.sin(alpha) * N[0],
        math.cos(alpha) * U[1] - math.sin(alpha) * N[1],
        math.cos(alpha) * U[2] - math.sin(alpha) * N[2]
    ]

    # 计算切点坐标
    P1 = [
        math.cos(r_rad) * C[0] + math.sin(r_rad) * V1[0],
        math.cos(r_rad) * C[1] + math.sin(r_rad) * V1[1],
        math.cos(r_rad) * C[2] + math.sin(r_rad) * V1[2]
    ]
    P2 = [
        math.cos(r_rad) * C[0] + math.sin(r_rad) * V2[0],
        math.cos(r_rad) * C[1] + math.sin(r_rad) * V2[1],
        math.cos(r_rad) * C[2] + math.sin(r_rad) * V2[2]
    ]

    # 归一化切点
    P1 = normalize_vector(P1)
    P2 = normalize_vector(P2)
    return P1, P2


def calculate_detour_waypoints(lon0 : float, lat0: float, lon_target: float, lat_target: float, lon_center: float, lat_center: float, radius: float):
    """
    计算导弹绕飞禁飞区的切点坐标
    :param lon0: 发射点经度（度）
    :param lat0: 发射点纬度（度）
    :param lon_target: 目标点经度（度）
    :param lat_target: 目标点纬度（度）
    :param lon_center: 禁飞区中心经度（度）
    :param lat_center: 禁飞区中心纬度（度）
    :param radius: 禁飞区半径（米）
    :return: 切点的经度, 纬度（度）
    """
    R = 6371000.0  # 地球半径（米）
    r_rad = (radius + 5000) / R  # 扩大后的禁飞区半径（弧度）

    # 将经纬度转换为弧度
    lon0_rad = math.radians(lon0)
    lat0_rad = math.radians(lat0)
    lon_target_rad = math.radians(lon_target)
    lat_target_rad = math.radians(lat_target)
    lon_center_rad = math.radians(lon_center)
    lat_center_rad = math.radians(lat_center)

    # 转换为笛卡尔坐标
    A = spherical_to_cartesian(lon0_rad, lat0_rad)  # 发射点
    B = spherical_to_cartesian(lon_target_rad, lat_target_rad)  # 目标点
    C = spherical_to_cartesian(lon_center_rad, lat_center_rad)  # 禁飞区中心

    try:
        # 获取发射点到禁飞区的切点
        P1, P2 = get_tangent_points(A, C, r_rad)

        # 计算两个切点到目标点的距离
        dist1 = great_circle_distance(P1, B)
        dist2 = great_circle_distance(P2, B)

        # 选择离目标点较近的切点
        T = P1 if dist1 < dist2 else P2

        # 将切点转换为经纬度（度）
        lon_rad, lat_rad = cartesian_to_spherical(T)
        waypoints = []
        waypoints.append((lon0, lat0))
        waypoints.append((math.degrees(lon_rad), math.degrees(lat_rad)))
        waypoints.append((lon_target, lat_target))
        return waypoints

    except ValueError as e:
        print(f"错误: {e}")
        return None, None


# 示例用法
if __name__ == "__main__":
    # 示例坐标（北京、上海、郑州禁飞区）
    lon0, lat0 = 49.323538, 7.924372  # 北京
    lon_target, lat_target = 60.621125, 8.183806  # 上海
    lon_center, lat_center = 53.1519,8.33533  # 郑州
    radius = 30000  # 禁飞区半径100公里

    # 计算切点
    waypoints = []

    waypoints = calculate_detour_waypoints(
        lon0, lat0, lon_target, lat_target, lon_center, lat_center, radius
    )
    print(waypoints)
    # if tangent_lon is not None:
    #     print(f"切点坐标: 经度={tangent_lon:.6f}, 纬度={tangent_lat:.6f}")
    # else:
    #     print("无法计算切点")